/**
  ******************************************************************************
  * @file           : pose_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/13
  ******************************************************************************
  */



#include "pose_task.h"

extern rc_device_t g_rc;
extern INS_t INS;
extern chassis_info chassisInfo;
extern float VisionChangeYaw;
extern uint8_t VisionControlFlag;
extern twins_receive_info twins_receive;
extern uint8_t enemy_aim_enable[12];
extern uint8_t armor_number;
extern float yaw_compensation;

GyroDataStruct GyroDataSave = {0};
StampDataStruct TimeStampedData = {0};

//陀螺仪控   弧度
float yaw_set_gyro = 0;
float pitch_set_gyro = 0;

float yaw_speed_gyro = 0;
float pitch_speed_gyro = 0;

float chassis_yaw_offset = 0;

uint8_t is_right_of_axis = 0; //是否在中轴线右侧

//角度
float YAW_GYRO_MAX = 0;
float YAW_GYRO_MIN = 0;

float PITCH_GYRO_MAX = 38.4;
float PITCH_GYRO_MIN = -31;

float YAW_GYRO_MINI_CAMERA_MAX = 0;
float YAW_GYRO_MINI_CAMERA_MIN = 0;

float YAW_GYRO_OFFSET = 0;   //左右头统一坐标系

float float2limit(float parameter, float limit){ //把浮点数限制到0-limit
    float float_parameter = parameter / limit;
    float_parameter = float_parameter - floor(float_parameter);
    return(float_parameter * limit);
}

void pitch_task(void *argument){
    static pid pitch_gyro_ctrl(0.8f, 0.1f, -3.5f, 0.f, 0.f);  //陀螺仪做位置控制
    static DJI_motor pitch_motor(&hcan1, nullptr, 2, DJI_GM6020, 1); //zyx坐标系，向下旋转为正
    pitch_motor.motor_change_speed_source(&pitch_speed_gyro);
    pitch_motor.velPid.pid_reset(1.0f, 0.1f, -22.f, 0.00f, 0.f);
    pitch_motor.posPid.pid_reset(0.8f, 0.1f, -3.5f, 0.f, 0.f);
    pitch_motor.motor_control(DJI_MOTOR_DISABLE_OFFSET);
    osDelay(5); //等待offset更新
    pitch_set_gyro = INS.Pitch / 180.f;
    for(;;){
        pitch_speed_gyro = INS.Gyro[Y] / 2.0f / PI * 60.0f;
        PITCH_GYRO_MAX = INS.Pitch + (PITCH_MAX - pitch_motor.motor_get_rounds()) * 360.f;
        PITCH_GYRO_MIN = INS.Pitch + (PITCH_MIN - pitch_motor.motor_get_rounds()) * 360.f;

        if(VisionControlFlag == 1 && enemy_aim_enable[armor_number] == 1){
            pitch_gyro_ctrl.pid_reset(1.0f, 0.1, -3.5f, 0.f, 0.f);
            pitch_motor.velPid.pid_reset(1.0f, 0.1f, -30.f, 0.00f, 0.f);
        }else{
            pitch_gyro_ctrl.pid_reset(1.0f, 0.1, -3.5f, 0.f, 0.f);
            pitch_motor.velPid.pid_reset(1.0f, 0.1f, -22.f, 0.00f, 0.f);
        }

        if(g_rc.is_lost == 0 && chassisInfo.is_flip_over == 0){
            //只有右边拨杆到中间的时候才增加
            pitch_set_gyro = pitch_set_gyro - (g_rc.rc_info.sw.sw_r == RC_SW_MID ? g_rc.rc_info.rocker.right_y / 5000.0 : 0);
            pitch_set_gyro = pitch_set_gyro > PITCH_GYRO_MAX / 180.f ? PITCH_GYRO_MAX / 180.f : pitch_set_gyro;
            pitch_set_gyro = pitch_set_gyro < PITCH_GYRO_MIN / 180.f ? PITCH_GYRO_MIN / 180.f : pitch_set_gyro;

            pitch_motor.motor_set_speed(pitch_gyro_ctrl.pid_calculate(pitch_set_gyro, INS.Pitch / 180.0f)); //使用陀螺仪控
//            pitch_motor.motor_set_current(0);

        }else{
            pitch_set_gyro = INS.Pitch / 180.f;
            pitch_motor.motor_set_current(0);
        }
        osDelay(1);
    }
}


void yaw_task(void *argument){
    static pid yaw_gyro_ctrl(1.0f, 0.1, 4.f, 0.f, 0.f);  //陀螺仪做位置控制
    static DJI_motor yaw_motor(&hcan1, nullptr, 1, DJI_GM6020, 0);
    yaw_motor.motor_change_speed_source(&yaw_speed_gyro);
    yaw_motor.velPid.pid_reset(1.0f, 0.1f, 6.f, 0.00f, 0.f);
    yaw_motor.motor_control(DJI_MOTOR_DISABLE_OFFSET);
    osDelay(5); //等待offset更新

    if(yaw_motor.motor_get_rounds() > YAW_MAX + 0.05){  //加点偏置
        yaw_motor.motor_control(DJI_MOTOR_ADD_ONE_LAP_OFFSET);
    }else if(yaw_motor.motor_get_rounds() < YAW_MIN - 0.05){
        yaw_motor.motor_control(DJI_MOTOR_SUB_ONE_LAP_OFFSET);
    }
    osDelay(5); //等待offset更新

    YAW_GYRO_OFFSET = (yaw_motor.motor_get_rounds() - YAW_FORWARD_ROUNDS) * 360.f;
    INS.My_Yaw_Origin = INS.YawTotalAngle + YAW_GYRO_OFFSET;
    INS.My_Yaw = INS.YawTotalAngle + YAW_GYRO_OFFSET + yaw_compensation;
    chassis_yaw_offset = (yaw_motor.motor_get_rounds() - YAW_FORWARD_ROUNDS) * 360.f;

    YAW_GYRO_MAX = INS.My_Yaw + (YAW_MAX - yaw_motor.motor_get_rounds()) * 360.f;
    YAW_GYRO_MIN = INS.My_Yaw + (YAW_MIN - yaw_motor.motor_get_rounds()) * 360.f;
    YAW_GYRO_MINI_CAMERA_MAX = INS.My_Yaw + (YAW_MINI_CAMERA_MAX - yaw_motor.motor_get_rounds()) * 360.f;
    YAW_GYRO_MINI_CAMERA_MIN = INS.My_Yaw + (YAW_MINI_CAMERA_MIN - yaw_motor.motor_get_rounds()) * 360.f;

    yaw_set_gyro = INS.My_Yaw / 360.f;
    for(;;) {
        yaw_speed_gyro = INS.Gyro[Z] / 2.0f / PI * 60.0f;
        INS.My_Yaw_Origin = INS.YawTotalAngle + YAW_GYRO_OFFSET;
        INS.My_Yaw = INS.YawTotalAngle + YAW_GYRO_OFFSET + yaw_compensation;
        chassis_yaw_offset = (yaw_motor.motor_get_rounds() - YAW_FORWARD_ROUNDS) * 360.f;
        YAW_GYRO_MAX = INS.My_Yaw + (YAW_MAX - yaw_motor.motor_get_rounds()) * 360.f;
        YAW_GYRO_MIN = INS.My_Yaw + (YAW_MIN - yaw_motor.motor_get_rounds()) * 360.f;
        YAW_GYRO_MINI_CAMERA_MAX = INS.My_Yaw + (YAW_MINI_CAMERA_MAX - yaw_motor.motor_get_rounds()) * 360.f;
        YAW_GYRO_MINI_CAMERA_MIN = INS.My_Yaw + (YAW_MINI_CAMERA_MIN - yaw_motor.motor_get_rounds()) * 360.f;
        is_right_of_axis = (yaw_motor.motor_get_rounds() - VisionChangeYaw / 360.f) < YAW_FORWARD_ROUNDS + YAW_RIGHT_OFFSET ? 1 : 0;

        if(VisionControlFlag == 1 && enemy_aim_enable[armor_number] == 1){
            yaw_gyro_ctrl.pid_reset(1.0, 0.1, 5, 0, 0);
            yaw_motor.velPid.pid_reset(1.0f, 0.1f, 10.f, 0.00f, 0.f);
        }else{
            yaw_gyro_ctrl.pid_reset(1.0f, 0.1, 4.f, 0.f, 0.f);
            yaw_motor.velPid.pid_reset(1.0f, 0.1f, 6.f, 0.00f, 0.f);
        }

        if (g_rc.is_lost == 0 && chassisInfo.is_flip_over == 0) {
            //只有右边拨杆到中间的时候才增加
            yaw_set_gyro = yaw_set_gyro - (g_rc.rc_info.sw.sw_r == RC_SW_MID ? g_rc.rc_info.rocker.right_x / 5000.0 : 0);
            yaw_set_gyro = yaw_set_gyro > YAW_GYRO_MAX / 360.f ? YAW_GYRO_MAX / 360.f : yaw_set_gyro;
            yaw_set_gyro = yaw_set_gyro < YAW_GYRO_MIN / 360.f ? YAW_GYRO_MIN / 360.f : yaw_set_gyro;

            yaw_motor.motor_set_speed(yaw_gyro_ctrl.pid_calculate(yaw_set_gyro, INS.My_Yaw / 360.0f));
//            yaw_motor.motor_set_current(0);

        }else{
            yaw_set_gyro = INS.My_Yaw / 360.f;
            yaw_motor.motor_set_current(0);
        }
        osDelay(1);
    }
}


void GyroDataGet(){
    GyroDataSave.Yaw = INS.My_Yaw / 360.f;
    GyroDataSave.Pitch = INS.Pitch / 180.f;
}